#include <ros/duration.h>
#include <control_toolbox/pid.h>

// 动态重配置
#include <dynamic_reconfigure/server.h>
#include <diffbot_base/ParametersConfig.h>
#include <boost/thread/mutex.hpp>

namespace diffbot_base
{
    class PID : public control_toolbox::Pid
    {
    public:
        /**
         * @brief 构造一个新的PID对象
         * 
         * @param p 比例增益
         * @param i 积分增益
         * @param d 微分增益
         * @param i_max 积分饱和上限
         * @param i_min 积分饱和下限
         * @param antiwindup 启用或禁用积分器反饱和
         * @param out_min 计算输出的最小值
         * @param out_max 计算输出的最大值
         */
        PID(double p=0.0, double i=0.0, double d=0.0, double i_max=0.0, double i_min=0.0, bool antiwindup=false, double out_max=0.0, double out_min=0.0);

        /**
         * @brief 初始化PID控制器
         * 
         * @param nh ROS节点句柄,可能包含描述PID控制器用途的命名空间
         * @param f 前馈增益
         * @param p 比例增益
         * @param i 积分增益
         * @param d 微分增益
         * @param i_max 积分饱和上限
         * @param i_min 积分饱和下限
         * @param antiwindup 反饱和标志
         * @param out_min 输出最小值
         * @param out_max 输出最大值
         */
        void init(ros::NodeHandle& nh, double f, double p, double i, double d, double i_max, double i_min, bool antiwindup, double out_max, double out_min);

        /**
         * @brief 使用过程值、设定值和时间周期从误差计算PID输出值
         * 
         * @param measured_value 传感器测量的过程值
         * @param setpoint 期望的目标值
         * @param dt 自上次调用以来的时间增量或周期
         * @return double 计算得到的PID输出值
         */
        double operator()(const double &measured_value, const double &setpoint, const ros::Duration &dt);

        /**
         * @brief 获取FPID参数
         * 
         * @param f 前馈增益
         * @param p 比例增益
         * @param i 积分增益
         * @param d 微分增益
         * @param i_max 积分饱和上限
         * @param i_min 积分饱和下限
         */
        void getParameters(double &f, double &p, double &i, double &d, double &i_max, double &i_min);
        /**
         * @brief 获取FPID参数
         * 
         * @param f 前馈增益
         * @param p 比例增益
         * @param i 积分增益
         * @param d 微分增益
         * @param i_max 积分饱和上限
         * @param i_min 积分饱和下限
         * @param antiwindup 启用或禁用反饱和检查
         */
        void getParameters(double &f, double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup);

        /**
         * @brief 使用control_toolbox::Pid的Gains对象设置参数
         * 
         * @param f 前馈增益
         * @param p 比例增益
         * @param i 积分增益
         * @param d 微分增益
         * @param i_max 积分饱和上限
         * @param i_min 积分饱和下限
         * @param antiwindup 启用或禁用反饱和检查
         */
        void setParameters(double f, double p, double i, double d, double i_max, double i_min, bool antiwindup = false);

        /**
         * @brief 设置PID控制器的输出限制
         * 
         * @param out_min 输出最小值
         * @param out_max 输出最大值
         */
        void setOutputLimits(double out_min, double out_max);

        /**
         * @brief 将给定值限制在上下限范围内
         * 
         * @param value 需要限制的输入值
         * @param lower_limit 值不能低于的下限
         * @param upper_limit 值不能超过的上限
         * @return double 限制在[lower_limit, upper_limit]范围内的值
         */
        double clamp(const double& value, const double& lower_limit, const double& upper_limit);

        /**
         * @brief 获取当前误差
         * 
         * @return double 从测量值和目标值计算得到的当前误差
         */
        inline double getError() const { return error_; };

        /**
         * @brief 启动动态重配置节点并加载默认值
         * @param node - 用于发布动态重配置服务的节点句柄
         */
        void initDynamicReconfig(ros::NodeHandle &node);

        /**
         * @brief 将动态重配置的增益设置为PID的值
         */
        void updateDynamicReconfig();
        void updateDynamicReconfig(Gains gains_config);
        void updateDynamicReconfig(diffbot_base::ParametersConfig config);

        /**
         * \brief 从动态重配置更新PID参数
         */
        void dynamicReconfigCallback(diffbot_base::ParametersConfig &config, uint32_t /*level*/);

    private:
        double f_;                // 前馈增益
        double error_;           // 误差值
        double out_min_;         // 输出最小值
        double out_max_;         // 输出最大值

        // 动态重配置
        bool dynamic_reconfig_initialized_;                                       // 动态重配置初始化标志
        typedef dynamic_reconfigure::Server<diffbot_base::ParametersConfig> DynamicReconfigServer;
        boost::shared_ptr<DynamicReconfigServer> param_reconfig_server_;          // 动态重配置服务器
        DynamicReconfigServer::CallbackType param_reconfig_callback_;             // 动态重配置回调

        boost::recursive_mutex param_reconfig_mutex_;                             // 动态重配置互斥锁
    };

}